/*
 * Copyright (c)  Inc.
 * Author: geremy yang
 * This node is used to read canet port N data and publish the data content, and
 * subscription data content into canet port N.
 * * *************************************************************************
 * */
#ifndef XZACTUATORNODE_H
#define XZACTUALORNODE_H

// #include "pthread.h"
#include "std_msgs/Float32MultiArray.h"
#include "xfcanetdriver/Boost_UDP_receive.h"
#include "xfcanetdriver/battery_msg.h"
#include "xfcanetdriver/canInfo.h"
#include "xfcanetdriver/can_debugging_msg.h"
#include "xfcanetdriver/controlresult.h"
#include "xfcanetdriver/reciv_light_msg.h"
#include "xfcanetdriver/ultrasonic_msg.h"
// #include "xfcanetdriver/diff_msg.h"
#include <iostream>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <string.h>

/* FOREGROUND color control*/

#define RST "\x1B[0m"
#define KRED "\x1B[31m"
#define KGRN "\x1B[32m"
#define KYEL "\x1B[33m"
#define KBLU "\x1B[34m"
#define KMAG "\x1B[35m"
#define KCYN "\x1B[36m"
#define KWHT "\x1B[37m"
#define FRED(x) KRED x RST
#define FGRN(x) KGRN x RST
#define FYEL(x) KYEL x RST
#define FBLU(x) KBLU x RST
#define FMAG(x) KMAG x RST
#define FCYN(x) KCYN x RST
#define FWHT(x) KWHT x RST
#define BOLD(x) "\x1B[1m" x RST
#define UNDL(x) "\x1B[4m" x RST

using namespace std;

typedef union CHAR2UINT // CHAR2UINT定义的变量只能选int 或者　char一种
{
  int i;
  char ch[4]; // union实际上是公用内存的，定义的变量按照最大内存为标准，数据共享
} CHAR2UINT;

typedef union Char2Uint
{
  unsigned short us;
  char ch[2];
} Char2Uint;

typedef union Char2Uchar
{
  unsigned char uc;
  char ch;
} Char2Uchar;

typedef union Char2Int
{
  int i;
  char ch[4];
} Char2Int;

typedef union Char2Ushort
{
  unsigned short sh;
  unsigned char ch[2];
} Char2Ushort;

typedef union Char2Short
{
  short sh;
  unsigned char ch[2];
} Char2Short;

// car ---> actuator
// 从CanetUdp那得到的CAN帧
typedef struct _CANMsg
{
  unsigned char head;    // message头文件
  unsigned ID;           // ID of message
  unsigned char data[8]; // Data of message
} stCANMsg;              // CAN报文定义

const static int RECVBUFFERSIZE = 13 * 100;

class xfcanetReceive
{
public:
  xfcanetReceive(ros::NodeHandle &handle);
  ~xfcanetReceive();
  void RecvCarInfoKernel();
  void ParserData(unsigned char data[], int num);
  void run();

public:
  int bdebug;
  std::string m_canetip;
  int m_canetport;
  std::string m_pcip;
  int m_pcport;

  ros::NodeHandle m_handle;
  // interface
  ros::Publisher pub_control;

  // data
  xfcanetdriver::canInfo car_info;

  // boost
  Boost_UDP_Receive *boost_udp;
  unsigned char buffer[RECVBUFFERSIZE];
};

#endif